/******************************************************************
Copyright © Deng Zhimao Co., Ltd. 2021-2030. All rights reserved.
* @projectName   camera_opencv_test
* @brief         cameraframethread.cpp
* @author        Deng Zhimao
* @email         dengzhimao@alientek.com/1252699831@qq.com
* @date          2024-03-25
* @link          http://www.openedv.com/forum.php
*******************************************************************/
#include "cameraframethread.h"
#include "opencv2/opencv.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/core/core.hpp"
#include <QDebug>
#include <vector>
#include <iostream>
#include <fstream>
#include <execution>


//#define PERFORM_DISTORTION_CORRECTION

#ifdef PERFORM_DISTORTION_CORRECTION
    #include <execution>
#endif

// 全局变量
cv::Mat image;
cv::Mat warped_image;
std::vector<cv::Point2f> points;
std::vector<cv::Point2f> default_points;

int selected_point = 0;
int step = 10;

// 读取保存的四个点坐标
void readPoints() {
    std::ifstream file("points.txt");
    if (file.is_open()) {
        points.clear();
        std::string line;
        while (std::getline(file, line)) {
            std::istringstream iss(line);
            float x, y;
            char comma;
            if (iss >> x >> comma >> y && comma == ',') {
                points.emplace_back(x, y);
            } else {
                std::cerr << "解析 points.txt 文件时出错，格式不正确。" << std::endl;
                break;
            }
        }
        file.close();
    } else {
        // 如果文件不存在，使用默认值
        points = default_points;
    }
}

// 保存点坐标的函数
void save_points(const std::vector<cv::Point2f>& pts) {
    std::ofstream file("points.txt");
    if (file.is_open()) {
        for (const auto& point : pts) {
            file << point.x << "," << point.y << std::endl;
        }
        file.close();
        std::cout << "四个点的坐标已保存到 points.txt" << std::endl;
    }
}

// 保存当前图片的函数
void save_current_image(const cv::Mat& img) {
    cv::imwrite("transformed_image.jpg", img);
    std::cout << "当前透视变换后的图片已保存为 transformed_image.jpg" << std::endl;
}

// 定义键盘控制函数
void on_key_press(int key) {
    switch (key) {
    case 'w': // 上移
        for (auto& point : points) {
            point.y = std::max(point.y - step, 0.0f);
        }
        break;
    case 's': // 下移
        for (auto& point : points) {
            point.y = std::min(point.y + step, static_cast<float>(image.rows));
        }
        break;
    case 'a': // 左移
        for (auto& point : points) {
            point.x = std::max(point.x - step, 0.0f);
        }
        break;
    case 'd': // 右移
        for (auto& point : points) {
            point.x = std::min(point.x + step, static_cast<float>(image.cols));
        }
        break;
    case '1': // 选择第一个点
        selected_point = 0;
        break;
    case '2': // 选择第二个点
        selected_point = 1;
        break;
    case '3': // 选择第三个点
        selected_point = 2;
        break;
    case '4': // 选择第四个点
        selected_point = 3;
        break;
    case 'i': // 所选点上移
        points[selected_point].y = std::max(points[selected_point].y - step, 0.0f);
        break;
    case 'k': // 所选点下移
        points[selected_point].y = std::min(points[selected_point].y + step, static_cast<float>(image.rows));
        break;
    case 'j': // 所选点左移
        points[selected_point].x = std::max(points[selected_point].x - step, 0.0f);
        break;
    case 'l': // 所选点右移
        points[selected_point].x = std::min(points[selected_point].x + step, static_cast<float>(image.cols));
        break;
    case 'r': // 恢复四个点的坐标为默认值
        points = default_points;
        std::cout << "四个点的坐标已恢复为默认值。" << std::endl;
        break;
    case 'q':
        std::cout << "退出程序。" << std::endl;
        break;
    case 'p':
        save_points(points);
        save_current_image(warped_image);
        break;
    }
}

// 定义透视变换函数
cv::Mat perform_perspective_transform(const cv::Mat& img, const std::vector<cv::Point2f>& pts) {
    std::vector<cv::Point2f> dst = {cv::Point2f(0, 0), cv::Point2f(1920, 0), cv::Point2f(1920, 1080), cv::Point2f(0, 1080)};
    cv::Mat M = cv::getPerspectiveTransform(dst, pts);
    cv::Mat warped;
    cv::warpPerspective(img, warped, M, cv::Size(1920, 1080));
    return warped;
}

#ifdef PERFORM_DISTORTION_CORRECTION
    // // 插值畸变矫正函数
    // void perform_distortion_correction(const cv::Mat& img,cv::Mat &target) {
    //     int width = img.cols;
    //     int height = img.rows;
    //     /*cv::Mat target(height, width, CV_8UC3, cv::Scalar(0, 0, 0));*/ // 默认是白色背景
    //     target.create(height, width, CV_8UC3); // 显式创建矩阵
    //     target.setTo(cv::Scalar(0, 0, 0)); // 设置默认背景为白色

    //     float a[9] = { 0.089, 0.9870, -0.0011, -0.0046, -0.0035, 0, 0.0011, 0, -0.0011 };
    //     float b[9] = { -0.0041, 0, 0.9461, -0.0016, 0.0288, 0.0115, -0.0068, 0, -0.0057 };
    //     std::vector<size_t> index(height * width);
    //     std::iota(index.begin(), index.end(), 0);

    //     std::for_each(index.begin(), index.end(), [width, height, a, b, &target, &img](size_t id) {
    //     //std::for_each(std::execution::par_unseq, index.begin(), index.end(), [width, height, a, b, &target, &img](size_t id) {
    //         int h = id / width;
    //         int w = id % width;
    //         float x = (w - width * 0.5f) / ((float)width * 0.5f);
    //         float y = (h - height * 0.5f) / ((float)height * 0.5f);
    //         float xy = x * y;
    //         float xx = x * x;
    //         float yy = y * y;
    //         float xxy = x * xy;
    //         float xyy = xy * y;
    //         float xxyy = xy * xy;
    //         float x_dis = a[0] + a[1] * x + a[2] * y + a[3] * xy + a[4] * xx + a[5] * yy + a[6] * xxy + a[7] * xyy + a[8] * xxyy;
    //         float y_dis = b[0] + b[1] * x + b[2] * y + b[3] * xy + b[4] * xx + b[5] * yy + b[6] * xxy + b[7] * xyy + b[8] * xxyy;
    //         float u = x_dis * width * 0.5f + width * 0.5f;
    //         float v = y_dis * height * 0.5f + height * 0.5f;
    //         if (u > 0 && u < width && v > 0 && v < height)
    //             target.at<cv::Vec3b>(h, w) = img.at<cv::Vec3b>(v, u);
    //     });
    // }
    // 插值畸变矫正函数
    void perform_distortion_correction(const cv::Mat& img, cv::Mat &target) {
        int width = img.cols;
        int height = img.rows;
        target.create(height, width, CV_8UC3); // 显式创建矩阵
        target.setTo(cv::Scalar(0, 0, 0)); // 设置默认背景为黑色
        float a[9] = { 0.089, 0.9870, -0.0011, -0.0046, -0.0035, 0, 0.0011, 0, -0.0011 };
        float b[9] = { -0.0041, 0, 0.9461, -0.0016, 0.0288, 0.0115, -0.0068, 0, -0.0057 };
        // float b[9] = { 0.010589248979591903 , 0.0006582857142859301, -0.006536326530612425, -0.0005845714285713926, -0.0005902040816327389, 0.0010694857142861114, 0.00449920000000005, 0.019701142857142966, 7.542857142854403e-05};
        // float a[9] = { 1.011685028571428 , -0.013686000000000167 , 0.0005345306122451789, -0.0007468571428576258, -0.08867965714285722, 0.5239761714285711, 0.11968114285714282, 0.005126857142857569, -0.020297142857142854 };
        cv::parallel_for_(cv::Range(0, height * width), [&](const cv::Range& range) {  //opencv并行计算的方式
            for (int id = range.start; id < range.end; ++id) {
                int h = id / width;
                int w = id % width;
                float x = (w - width * 0.5f) / ((float)width * 0.5f);
                float y = (h - height * 0.5f) / ((float)height * 0.5f);
                float xy = x * y;
                float xx = x * x;
                float yy = y * y;
                float xxy = x * xy;
                float xyy = xy * y;
                float xxyy = xy * xy;
                float x_dis = a[0] + a[1] * x + a[2] * y + a[3] * xy + a[4] * xx + a[5] * yy + a[6] * xxy + a[7] * xyy + a[8] * xxyy;
                float y_dis = b[0] + b[1] * x + b[2] * y + b[3] * xy + b[4] * xx + b[5] * yy + b[6] * xxy + b[7] * xyy + b[8] * xxyy;
                float u = x_dis * width * 0.5f + width * 0.5f;
                float v = y_dis * height * 0.5f + height * 0.5f;
                if (u > 0 && u < width && v > 0 && v < height)
                    target.at<cv::Vec3b>(h, w) = img.at<cv::Vec3b>(v, u);
            }
        });
    }
#endif

CameraFrameThread::CameraFrameThread(QObject *parent): QThread(parent)
{

}

void CameraFrameThread::run()
{
    // 读取图片（此处图片读取部分未使用，保留原代码结构）
    image = cv::imread("./point_matrix.png");
    if (image.empty())
    {
        std::cout << "无法读取图片，请检查图片路径。" << std::endl;
    }

    // 初始化四边形的四个顶点坐标，初始为 1920x1080 的长方形
    int width = image.cols;
    int height = image.rows;
    default_points = {cv::Point2f(0, 0), cv::Point2f(width, 0), cv::Point2f(width, height), cv::Point2f(0, height)};

    // 尝试读取保存的四个点坐标
    readPoints();

    cv::VideoCapture cap(40); // RK3568 HDMIIN是40
    // cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
    // cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);

    if (!cap.isOpened()) {
        qDebug() << "Camera open failed!";
        return;
    }

    cv::Mat target;
    while (true) {
        cv::Mat frame;
        cap.read(frame);
        // 检查是否成功读取图像可
        if (frame.empty()) {
            std::cout << "无法获取图像帧" << std::endl;
            break;
        }
        auto start = std::chrono::high_resolution_clock::now();
        //
#ifdef PERFORM_DISTORTION_CORRECTION
        perform_distortion_correction(frame,target);
        warped_image = target;
#else
        warped_image = perform_perspective_transform(frame, points);
#endif
        auto end = std::chrono::high_resolution_clock::now();
        // 计算时间间隔（毫秒）
        auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
        qDebug() << "执行时间：" << duration << "ms";
        // 使用深拷贝创建 QImage
        QImage tmpImage = QImage(warped_image.data, warped_image.cols, warped_image.rows, warped_image.step, QImage::Format_BGR888).copy();
        if(!tmpImage.isNull())
            emit imageIsReady(tmpImage);
        // 处理待处理的键盘事件
        if (pendingKey != -1) {
            on_key_press(pendingKey);
            if (pendingKey == 'q') {
                break;
            }
            pendingKey = -1;
        }
    }
    cap.release();
}

void CameraFrameThread::handleKeyPress(int key) {
    pendingKey = key;
}






// /******************************************************************
// Copyright © Deng Zhimao Co., Ltd. 2021-2030. All rights reserved.
// * @projectName   camera_opencv_test
// * @brief         cameraframethread.cpp
// * @author        Deng Zhimao
// * @email         dengzhimao@alientek.com/1252699831@qq.com
// * @date          2024-03-25
// * @link          http://www.openedv.com/forum.php
// *******************************************************************/
// #include "cameraframethread.h"
// #include "opencv2/opencv.hpp"
// #include "opencv2/imgproc.hpp"
// #include "opencv2/imgcodecs.hpp"
// #include "opencv2/core/core.hpp"
// #include <QDebug>
// #include <vector>
// #include <iostream>
// #include <fstream>

// // 全局变量
// cv::Mat image;
// cv::Mat warped_image;
// std::vector<cv::Point2f> points;
// std::vector<cv::Point2f> default_points;
// int selected_point = 0;
// int step = 10;
// // 读取保存的四个点坐标
// void readPoints() {
//     std::ifstream file("points.txt");
//     if (file.is_open()) {
//         points.clear();
//         std::string line;
//         while (std::getline(file, line)) {
//             std::istringstream iss(line);
//             float x, y;
//             char comma;
//             if (iss >> x >> comma >> y && comma == ',') {
//                 points.emplace_back(x, y);
//             } else {
//                 std::cerr << "解析 points.txt 文件时出错，格式不正确。" << std::endl;
//                 break;
//             }
//         }
//         file.close();
//     } else {
//         // 如果文件不存在，使用默认值
//         points = default_points;
//     }
// }
// // 保存点坐标的函数
// void save_points(const std::vector<cv::Point2f>& pts) {
//     std::ofstream file("points.txt");
//     if (file.is_open()) {
//         for (const auto& point : pts) {
//             file << point.x << "," << point.y << std::endl;
//         }
//         file.close();
//         std::cout << "四个点的坐标已保存到 points.txt" << std::endl;
//     }
// }


// // 保存当前图片的函数
// void save_current_image(const cv::Mat& img) {
//     cv::imwrite("transformed_image.jpg", img);
//     std::cout << "当前透视变换后的图片已保存为 transformed_image.jpg" << std::endl;
// }
// // 定义键盘控制函数
// void on_key_press(int key) {
//     switch (key) {
//     case 'w': // 上移
//         for (auto& point : points) {
//             point.y = std::max(point.y - step, 0.0f);
//         }
//         break;
//     case 's': // 下移
//         for (auto& point : points) {
//             point.y = std::min(point.y + step, static_cast<float>(image.rows));
//         }
//         break;
//     case 'a': // 左移
//         for (auto& point : points) {
//             point.x = std::max(point.x - step, 0.0f);
//         }
//         break;
//     case 'd': // 右移
//         for (auto& point : points) {
//             point.x = std::min(point.x + step, static_cast<float>(image.cols));
//         }
//         break;
//     case '1': // 选择第一个点
//         selected_point = 0;
//         break;
//     case '2': // 选择第二个点
//         selected_point = 1;
//         break;
//     case '3': // 选择第三个点
//         selected_point = 2;
//         break;
//     case '4': // 选择第四个点
//         selected_point = 3;
//         break;
//     case 'i': // 所选点上移
//         points[selected_point].y = std::max(points[selected_point].y - step, 0.0f);
//         break;
//     case 'k': // 所选点下移
//         points[selected_point].y = std::min(points[selected_point].y + step, static_cast<float>(image.rows));
//         break;
//     case 'j': // 所选点左移
//         points[selected_point].x = std::max(points[selected_point].x - step, 0.0f);
//         break;
//     case 'l': // 所选点右移
//         points[selected_point].x = std::min(points[selected_point].x + step, static_cast<float>(image.cols));
//         break;
//     case 'r': // 恢复四个点的坐标为默认值
//         points = default_points;
//         std::cout << "四个点的坐标已恢复为默认值。" << std::endl;
//         break;
//     }
// }
// // 定义透视变换函数
// cv::Mat perform_perspective_transform(const cv::Mat& img, const std::vector<cv::Point2f>& pts) {
//     std::vector<cv::Point2f> dst = {cv::Point2f(0, 0), cv::Point2f(1920, 0), cv::Point2f(1920, 1080), cv::Point2f(0, 1080)};
//     cv::Mat M = cv::getPerspectiveTransform(dst, pts);
//     cv::Mat warped;
//     cv::warpPerspective(img, warped, M, cv::Size(1920, 1080));
//     return warped;
// }


// CameraFrameThread::CameraFrameThread(QObject *parent): QThread(parent)
// {

// }

// void CameraFrameThread::run()
// {
//     // 读取图片（此处图片读取部分未使用，保留原代码结构）
//     image = cv::imread("./point_matrix.png");
//     if (image.empty())
//     {
//         std::cout << "无法读取图片，请检查图片路径。" << std::endl;
//     }

//     // 初始化四边形的四个顶点坐标，初始为 1920x1080 的长方形
//     int width = image.cols;
//     int height = image.rows;
//     default_points = {cv::Point2f(0, 0), cv::Point2f(width, 0), cv::Point2f(width, height), cv::Point2f(0, height)};

//     // 尝试读取保存的四个点坐标
//     readPoints();



//     cv::VideoCapture cap(40); // RK3568MIPI摄像头是0 usb摄像头是9。其他板子请参考
//     // cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
//     // cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);

//     if (!cap.isOpened()) {
//         qDebug() << "Camera open failed!";
//         return;
//     }

//     while (true) {
//         cv::Mat frame;
//         cap.read(frame);

//         // 检查是否成功读取图像
//         if (frame.empty()) {
//             std::cout << "无法获取图像帧" << std::endl;
//             break;
//         }

//         cv::Mat warped_image = perform_perspective_transform(frame, points);

//         QImage tmpImage(warped_image.data, warped_image.cols, warped_image.rows, QImage::Format_BGR888);
//         if(!tmpImage.isNull())
//             emit imageIsReady(tmpImage);

//         // 等待键盘输入
//         int key = cv::waitKey(1);
//         if (key == -1) {  // 无按键输入
//             continue;
//         }
//         // 按 'q' 键退出循环
//         if (key == 'q') {
//             break;
//         }
//         // 按 'p' 键保存四个点的坐标和当前透视变换后的图片
//         if (key == 'p') {
//             save_points(points);
//             save_current_image(warped_image);
//         }
//         // 处理键盘输入
//         on_key_press(key);
//     }
//     cap.release();
// }










// // /******************************************************************
// // Copyright © Deng Zhimao Co., Ltd. 2021-2030. All rights reserved.
// // * @projectName   camera_opencv_test
// // * @brief         cameraframethread.cpp
// // * @author        Deng Zhimao
// // * @email         dengzhimao@alientek.com/1252699831@qq.com
// // * @date          2024-03-25
// // * @link          http://www.openedv.com/forum.php
// // *******************************************************************/
// // #include "cameraframethread.h"
// // #include "opencv2/opencv.hpp"
// // #include "opencv2/imgproc.hpp"
// // #include "opencv2/imgcodecs.hpp"
// // #include "opencv2/core/core.hpp"
// // #include <QDebug>




// // // 定义透视变换函数
// // cv::Mat perform_perspective_transform(const cv::Mat& img, const std::vector<cv::Point2f>& pts) {
// //     std::vector<cv::Point2f> dst = {cv::Point2f(0, 0), cv::Point2f(1920, 0), cv::Point2f(1920, 1080), cv::Point2f(0, 1080)};
// //     cv::Mat M = cv::getPerspectiveTransform(dst, pts);
// //     cv::Mat warped;
// //     cv::warpPerspective(img, warped, M, cv::Size(1920, 1080));
// //     return warped;
// // }

// // CameraFrameThread::CameraFrameThread(QObject *parent): QThread(parent)
// // {

// // }

// // void CameraFrameThread::run()
// // {
// //     cv::VideoCapture cap(40); // RK3568MIPI摄像头是0 usb摄像头是9。其他板子请参考
// //     // cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
// //     // cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);

// //     if (!cap.isOpened()) {
// //         qDebug() << "Camera open failed!";
// //         return;
// //     }

// //     while (true) {
// //         cv::Mat frame;
// //         cap.read(frame);
// //         //cv::Mat warped_image = perform_perspective_transform(frame, points);
// //         QImage tmpImage(frame.data, frame.cols, frame.rows, QImage::Format_BGR888);
// //         if(!tmpImage.isNull())
// //             emit imageIsReady(tmpImage);
// //     }
// //     cap.release();
// // }
